
% Input
% process_var: process variance
% T: time interval
% gyro_x, gyro_y, gyro_z: gyro data in rad/s
% acc_x, acc_y, acc_z
% mag_x, mag_y, mag_z
% ax_ref, ay_ref, az_ref: accelerometer reference data
% mx_ref, my_ref, mz_ref: magnetometer reference data
% acc_var: accelerometer sensor covariance
% mag_var: magnetometer sensor covariance
% process_var

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


clc;
clear all;

%% Set up the figure 


%serial code


numSec=1;
t=[];
v=[];
%ACCLERATION REFERENCE:  x = 0.00593512389196179  
%                        y = 0.0587215182616255   
%                        z = 0.998233616389715
ax_ref = 0.00593;  
ay_ref = 0.05872;   
az_ref = -0.99823;
 
%GYRO REFERENCE:       
%                        x = 446.448215330603         
%                        y = 490.442949093037
%                        z = 407.889994148625
gyrorx = 446.44821;         
gyrory = 490.44294;
gyrorz = 407.88999;

% magnetometer reference
mx_ref = -0.721736182824162;
my_ref = 0.206610314726653;
mz_ref = -0.660612640093241;
 
%SERIAL PORT
s1 = serial('com5');    % define serial port
s1.BaudRate=115200;               % define baud rate
set(s1, 'terminator', 'CR');    % define the terminator for println
fopen(s1);
%END SERIAL PORT
%DEFINES
%1 degree = 0.0174532925 radian
deg2rad = 0.0174532925;
iteration = 1;
                         
                             
 t0 = clock;
 g = 0;
 
 %end serial

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Initialize
% Input: process_var
% Euler Angle
phi = 0;
theta = 0;
psi = 0;

% SET PROCESS VAR
process_var = 0.1 ;
acc_var = 2.0;
mag_var = 2.0;

% predicted estimate covariance
P = [process_var*0.00001, 0, 0, 0;
     0, process_var*0.00001, 0, 0;
     0, 0, process_var*0.00001, 0;
     0, 0, 0, process_var*0.00001];
 
% Innovation (or residual) covariance
R = [process_var*0.00001, 0, 0, 0;
     0, process_var*0.00001, 0, 0;
     0, 0, process_var*0.00001, 0;
     0, 0, 0, process_var*0.00001];

% Quaternion matrix
quaternion = [1 0 0 0];

zMag = [0 0 0];
while(true)
    T = etime(clock,t0);
    t0 = clock;
   
    w=fscanf(s1, '%d %d %d %d %d %d %d %d %d');              % must define the input % d or %s, etc.
  
    
    Acc = [w(1) w(2) -w(3)];
    Acc = Acc/norm(Acc);
    acc_x = Acc(1);
    acc_y = Acc(2);
    acc_z = Acc(3);
   
    %gyro data - this case is 9.1mV/degree, we are at 3.3V, 1024 steps,
    %each step is 3300/1024 = 3.22265625mV/step -> 0.354138049 degree/step
   
    %correct gyro with reference (need temp correction here), multiply by
    %step to get degrees, and then multiply by rad
    Gyro = ([w(4) w(5) w(6)] - [gyrorx gyrory gyrorz]) * 0.354138049 * 2.57 * deg2rad;
    gyro_x = Gyro(1);
    gyro_y = Gyro(2);
    gyro_z = Gyro(3);
    
 
    Mag = [w(7) w(8) w(9)];
    
    %zMag = (zMag + Mag);
    
    %zMag/iteration
    
    Mag = Mag/norm(Mag);
    mag_x = Mag(1);
    mag_y = Mag(2);
    mag_z = Mag(3);
    
 
    
    
    %code here..

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% EKF Predict
% Input: gyro_x, gyro_y, gyro_z, T
% Updates: Predicted State Estimate: phi, theta, psi
% Predicted estimate covariance: P
% state quaternion: quaternion
p = gyro_x;
q = gyro_y;
r = gyro_z;

% save current quaternion
a = quaternion(1);
b = quaternion(2);
c = quaternion(3);
d = quaternion(4);

% rotation rate quaternion
pqr_quat = [0 p q r];

% predict new quaternion state based on gyro data

% quaternion multiplication
%quaternion = quaternion + 0.5 * T * (pqr_quat * quaternion);
quaternion = quaternion + 0.5 * T * (quatmultiply(pqr_quat, quaternion));

% normalize quaternion
quaternion = quatnormalize(quaternion);

% calculate jacobian matrix F
F = [     1, -(T*p)/2, -(T*q)/2, -(T*r)/2;
    (T*p)/2,        1,  (T*r)/2, -(T*q)/2; 
    (T*q)/2, -(T*r)/2,        1,  (T*p)/2;
    (T*r)/2,  (T*q)/2, -(T*p)/2,        1];

% calculate NEW estimate covariance
P = F * P * (F') + R;

% calculate new euler angles based on new quaternion
q0 = quaternion(1);
q1 = quaternion(2);
q2 = quaternion(3);
q3 = quaternion(4);

phi = atan2(2*(q0*q1 + q2*q3),q3*q3 - q2*q2 - q1*q1 + q0*q0)*180/3.14159;
theta = -asin(2*(q1*q3 - q0*q2))*180/3.14159;
psi = atan2(2*(q0*q3+q1*q2),q1*q1 + q0*q0 - q3*q3 - q2*q2)*180/3.14159; 



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% EKF Update
% Input: acc_x, acc_y, acc_z, mag_x, mag_y, mag_z, ax_ref, ay_ref, az_ref,
% acc_var, mag_var


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Acceleromater
% x axis
% save current quaternion
a = quaternion(1);
b = quaternion(2);
c = quaternion(3);
d = quaternion(4);

% computer expected acc measurements based on quaternion and reference
ax_hat = ay_ref*(2*a*d + 2*b*c) - az_ref*(2*a*c - 2*b*d) + ax_ref*(a*a + b*b - c*c - d*d);

H = [ 2*a*ax_ref - 2*az_ref*c + 2*ay_ref*d 
    2*ax_ref*b + 2*ay_ref*c + 2*az_ref*d  
    2*ay_ref*b - 2*a*az_ref - 2*ax_ref*c  
    2*a*ay_ref + 2*az_ref*b - 2*ax_ref*d];

H = H';
% compute Kalman Gain
temp = H * P * (H');
K = P * (H') * (1/ (temp(1,1) + acc_var));

error = acc_x - ax_hat;

% update quaternion
quaternion = quaternion + (K * error)';

% update estimate covariance
P = (eye(4) - K * H)*P;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% y axis
% save current quaternion
a = quaternion(1);
b = quaternion(2);
c = quaternion(3);
d = quaternion(4);

% computer expected acc measurements based on quaternion and reference
ay_hat = az_ref*(2*a*b + 2*c*d) - ax_ref*(2*a*d - 2*b*c) + ay_ref*(a*a - b*b + c*c - d*d);

H = [ 2*a*ay_ref + 2*az_ref*b - 2*ax_ref*d, 
    2*a*az_ref - 2*ay_ref*b + 2*ax_ref*c, 
    2*ax_ref*b + 2*ay_ref*c + 2*az_ref*d, 
    2*az_ref*c - 2*a*ax_ref - 2*ay_ref*d];

H = H';

% compute Kalman Gain
temp = H * P * (H');
K = P * (H') * (1/ (temp(1,1) + acc_var));

error = acc_y - ay_hat;

% update quaternion
quaternion = quaternion + (K * error)';

% update estimate covariance
P = (eye(4) - K * H)*P;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% z axis
% save current quaternion
a = quaternion(1);
b = quaternion(2);
c = quaternion(3);
d = quaternion(4);

% computer expected acc measurements based on quaternion and reference
az_hat = ax_ref*(2*a*c + 2*b*d) - ay_ref*(2*a*b - 2*c*d) + az_ref*(a*a - b*b - c*c + d*d);

H = [2*a*az_ref - 2*ay_ref*b + 2*ax_ref*c, 
    2*ax_ref*d - 2*az_ref*b - 2*a*ay_ref, 
    2*a*ax_ref - 2*az_ref*c + 2*ay_ref*d, 
    2*ax_ref*b + 2*ay_ref*c + 2*az_ref*d];

H = H';
% compute Kalman Gain
temp = H * P * (H');
K = P * (H') * (1/ (temp(1,1) + acc_var));

error = acc_z - az_hat;

% update quaternion
quaternion = quaternion + (K * error)';

% update estimate covariance
P = (eye(4) - K * H)*P;
% 
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% % Magnetometer
% % x axis
% % save current quaternion
% a = quaternion(1);
% b = quaternion(2);
% c = quaternion(3);
% d = quaternion(4);
% 
% % computer expected acc measurements based on quaternion and reference
% mx_hat = my_ref*(2*a*d + 2*b*c) - mz_ref*(2*a*c - 2*b*d) + mx_ref*(a*a + b*b - c*c - d*d);
% 
% H = [ 2*a*mx_ref - 2*mz_ref*c + 2*my_ref*d, 
%     2*mx_ref*b + 2*my_ref*c + 2*mz_ref*d, 
%     2*my_ref*b - 2*a*mz_ref - 2*mx_ref*c, 
%     2*a*my_ref + 2*mz_ref*b - 2*mx_ref*d];
% 
% H = H';
% % compute Kalman Gain
% temp = H * P * (H');
% K = P * (H') * (1/ (temp(1,1) + mag_var));
% 
% error = mag_x - mx_hat;
% 
% % update quaternion
% quaternion = quaternion + (K * error)';
% 
% % update estimate covariance
% P = (eye(4) - K * H)*P;
% 
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% % y axis
% % save current quaternion
% a = quaternion(1);
% b = quaternion(2);
% c = quaternion(3);
% d = quaternion(4);
% 
% % computer expected acc measurements based on quaternion and reference
% my_hat = mz_ref*(2*a*b + 2*c*d) - mx_ref*(2*a*d - 2*b*c) + my_ref*(a*a - b*b + c*c - d*d);
% 
% H = [ 2*a*my_ref + 2*mz_ref*b - 2*mx_ref*d, 
%     2*a*mz_ref - 2*my_ref*b + 2*mx_ref*c, 
%     2*mx_ref*b + 2*my_ref*c + 2*mz_ref*d, 
%     2*mz_ref*c - 2*a*mx_ref - 2*my_ref*d];
% 
% H = H';
% % compute Kalman Gain
% temp = H * P * (H');
% K = P * (H') * (1/ (temp(1,1) + mag_var));
% 
% error = mag_y - my_hat;
% 
% % update quaternion
% quaternion = quaternion + (K * error)';
% 
% % update estimate covariance
% P = (eye(4) - K * H)*P;
% 
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% % z axis
% % save current quaternion
% a = quaternion(1);
% b = quaternion(2);
% c = quaternion(3);
% d = quaternion(4);
% 
% % computer expected acc measurements based on quaternion and reference
% mz_hat = mx_ref*(2*a*c + 2*b*d) - my_ref*(2*a*b - 2*c*d) + mz_ref*(a*a - b*b - c*c + d*d);
% 
% H = [2*a*mz_ref - 2*my_ref*b + 2*mx_ref*c, 
%     2*mx_ref*d - 2*mz_ref*b - 2*a*my_ref, 
%     2*a*mx_ref - 2*mz_ref*c + 2*my_ref*d, 
%     2*mx_ref*b + 2*my_ref*c + 2*mz_ref*d];
% 
% H = H';
% % compute Kalman Gain
% temp = H * P * (H');
% K = P * (H') * (1/ (temp(1,1) + mag_var));
% 
% error = mag_z - mz_hat;
% 
% % update quaternion
% quaternion = quaternion + (K * error)';
% 
% % update estimate covariance
% P = (eye(4) - K * H)*P;
% 
% % DONE, convert to euler angle and plot
% q0 = quaternion(1);
% q1 = quaternion(2);
% q2 = quaternion(3);
% q3 = quaternion(4);

phi  = atan2(2*(q0*q1 + q2*q3),q3*q3 - q2*q2 - q1*q1 + q0*q0)*180/3.14159;
theta = -asin(2*(q1*q3 - q0*q2))*180/3.14159;
psi = atan2(2*(q0*q3+q1*q2),q1*q1 + q0*q0 - q3*q3 - q2*q2)*180/3.14159;

v = [phi; theta; psi;]
    
    iteration = iteration + 1;
  
end
try
catch exception
    fclose(s1);                 % always, always want to close s1
    throw (exception);
end                            
 